#include "dkglMovable.h"
#include "dkglVector3.h"
#include "dkglDrawList.h"
#include "dkglAxes.h"

namespace DKGL
{
    Movable::Movable(const String& name):
        _name(name),
        _parent(NULL),
        _drawable(NULL),
        _axes(NULL),
		_orientation(Quaternion::IDENTITY),
		_translation(Vector3::ZERO)
    {
        SetIdentityMatrix(_globalMatrixCache);
    }

    Movable::~Movable()
    {
        // free children
        _childList.clear();

        if (_drawable)
            delete _drawable;

        if (_axes)
            delete _axes;
    }

    void Movable::rotate(float degree, const Vector3& axis,TRANS_SPACE space)
    {
        rotate(degree, axis.x, axis.y, axis.z, space);
    }


    //---------------------------------------------------------------------------
    void Movable::rotate(float degree, float x, float y, float z, TRANS_SPACE space)
    {
		Quaternion q(Degree2Radian(degree), Vector3(x,y,z));

	    if(space==LOCAL)
	    {	
			_orientation = _orientation * q;
	    }
	    else if(space==GLOBAL)
	    {
		    _orientation = q * _orientation;
	    }
        else
        {
            throw std::invalid_argument("");
        }
		_dirty = true;
    }


    void Movable::translate(const Vector3& axis, TRANS_SPACE space)
    {
        translate(axis.x, axis.y, axis.z, space);
    }
	
    //----------------------------------------------------------------------
    void Movable::translate(float x, float y, float z, TRANS_SPACE space)
    {
		Vector3 t(x, y, z);

	    if (space==LOCAL)
	    {	
			_translation += _orientation * t;
	    }
	    else if ( space == GLOBAL )
	    {
		    _translation += t;
	    }
        else
        {
            throw std::invalid_argument("");
        }
		_dirty = true;
    }//Endof 
	    
    //--------------------------------------------------
    void Movable::scale(const Vector3& s)
    {
        scale(s.x, s.y, s.z);
    }

    void Movable::scale(float x, float y, float z)
    {
        //TODO implement
        assert(0);
		_dirty = true;
    }
	
    //------------------------------------------------------- 
    void Movable::moveForward(float dist)
    {
        // Since -Z is the face direction
        translate(0, 0, -dist, LOCAL);
    }
	    
    void Movable::moveRight(float dist)
    {
        // right hand is positive X is the right 
        translate(dist, 0, 0, LOCAL);
    }
	    
    void Movable::moveUp(float dist)
    {
        translate(0, dist, 0, LOCAL);
    }

    void Movable::roll(float degree)
    {
        rotate(degree, 0.0f, 0.0f, 1.0f, LOCAL);
    }
	    
    void Movable::yaw(float degree)
    {
        rotate(degree, 0.0f, 1.0f, 0.0f, LOCAL);
    }
	    
    void Movable::pitch(float degree)
    {
        rotate(degree, 1.0f, 0.0f, 0.0f, LOCAL);
    }

    void Movable::setPosition(const Vector3& pos)
    {
        _translation = pos;
    }


    void Movable::setPosition(float x, float y, float z)
    {
        _translation.x = x;
		_translation.y = y;
		_translation.z = z;
    }
	    
    Vector3 Movable::getPosition() const
    {
        return _translation;
    }

	void Movable::lookAt(float x, float y, float z)
	{
		/*Vector3 faceDir = Vector3(x, y, z) - _translation;
		
		faceDir.normalise();

		Vector3 rightHand = faceDir.crossProduct(
			Vector3(_localMatrix[1], _localMatrix[5], _localMatrix[9]));

		Vector3 up = rightHand.crossProduct(faceDir);

		rightHand.normalise();
		up.normalise();

		_localMatrix[0] = rightHand.x;
		_localMatrix[4] = rightHand.y;
		_localMatrix[8] = rightHand.z;

		_localMatrix[1] = up.x;
		_localMatrix[5] = up.y;
		_localMatrix[9] = up.z;

		_localMatrix[2] = -faceDir.x;
		_localMatrix[6] = -faceDir.y;
		_localMatrix[10] = -faceDir.z;*/
	}
	    
    //-------------------------------------------------
    Vector3 Movable::getWorldPosition() const
    {
        assert(0);

        return Vector3::ZERO;
    }

    void Movable::addChild(MovablePtr child)
    {
		child->setParent(this);
        _childList.push_back(child);
    }

    MovablePtr Movable::removeChild(const String& name)
    {
        assert(0);

        MovablePtr dummy;

        return dummy;
    }

        
    MovablePtr Movable::getChild(const String& name)
    {
        assert(0);
        MovablePtr dummy;
        return dummy;

    }

    void Movable::getParentTransformation(float* matrix) const
    {
        assert(matrix);

        glMatrixMode(GL_MODELVIEW);
	    if(_parent)
	    {
		    _parent->getParentTransformation(matrix);
		    glPushMatrix();
		    glLoadMatrixf(matrix);
		    glMultMatrixf(_localMatrix);
		    glGetFloatv(GL_MODELVIEW_MATRIX, matrix);
		    glPopMatrix();
	    }
	    else
	    {
		    CopyMatrix(matrix, _localMatrix);
	    }
    }
	    
        
    void Movable::getParentRotationPart(float*) const
    {
        assert(0);

    }

    //--------------------------------------------------
    const String& Movable::getName(void) const
    {
        return _name;
    }


    //-------------------------------------------
    void Movable::updateTransform()
    {
		if (!_dirty)
			return;

        float globalMatrix[16];
	    glGetFloatv( GL_MODELVIEW_MATRIX , globalMatrix );

	    _transformation();
        _updateChildrenTransform();
            
	    glLoadMatrixf(globalMatrix);

		_dirty = false;
    }


    void Movable::_transformation()
    {
        glMultMatrixf(_localMatrix);
        glGetFloatv(GL_MODELVIEW_MATRIX, _globalMatrixCache);

        if (_drawable)
        {
            _drawable->setWorldTransformation(_globalMatrixCache);
			_drawable->render();
        }

        if (_axes)
        {
            _axes->setWorldTransformation(_globalMatrixCache);
			_axes->render();
        }
	}


    void Movable::_updateChildrenTransform()
    {
        ChildListIter iter(_childList.begin());
        ChildListIter end(_childList.end());
        for (;iter!=end;++iter)
        {
            (*iter)->updateTransform();
        }
    }

    void Movable::showAxes(bool b)
    {
        if (b)
        {
            if (!_axes)
                _axes = new Primitive::Axes();
        }
        else
        {
            if (_axes)
            {
                delete _axes;
                _axes = NULL;
            }
        }
    }

}//namespace